using System;
using System.Collections.Generic;
using System.Text;

namespace MiniBall
{
    class Basis
    {
        // types
        //typedef Wrapped_array<d>            Point;

        // data members
        private int m, s;   // size and number of support points
        private double[] q0;

        private double[] z;
        private double[] f;
        private double[][] v;
        private double[][] a;

        private double[][] c;
        private double[] sqr_r;

        private int current_c;      // points to some c[j]
        private double current_sqr_r;
        private int dimensions;

        public Basis(int dim)
        {
            dimensions = dim;

            q0 = new double[dimensions];

            z = new double[dimensions + 1];
            f = new double[dimensions + 1];
            v = new double[dimensions + 1][];
            for (int i = 0; i < v.Length; i++) v[i] = new double[dimensions];
            a = new double[dimensions + 1][];
            for (int i = 0; i < a.Length; i++) a[i] = new double[dimensions];

            c = new double[dimensions + 1][];
            for (int i = 0; i < c.Length; i++) c[i] = new double[dimensions];
            sqr_r = new double[dimensions + 1];

            Reset();

        }

        private double Sqr(double r)
        {
            return r * r;
        }

        public double[] Center
        {
            get
            {
                return c[current_c];
            }
        }

        public double SquaredRadius
        {
            get
            {
                return current_sqr_r;
            }
        }

        public int Size
        {
            get
            {
                return m;
            }
        }

        public int SupportSize
        {
            get
            {
                return s;
            }
        }

        public double Excess(double[] point)
        {
            double e = -current_sqr_r;
            for (int k = 0; k < point.Length; ++k)
                e += Sqr(point[k] - c[current_c][k]);
            return e;
        }

        public void Reset()
        {
            m = s = 0;
            // we misuse c[0] for the center of the empty sphere
            for (int j = 0; j < dimensions; ++j)
                c[0][j] = 0;
            current_c = 0;
            current_sqr_r = -1;
        }

        public void Pop()
        {
            --m;
        }

        public bool Push(double[] point)
        {
            int i, j;
            double eps = 1e-32f;
            if (m == 0)
            {
                for (i = 0; i < q0.Length; ++i)
                    q0[i] = point[i];
                for (i = 0; i < q0.Length; ++i)
                    c[0][i] = q0[i];
                sqr_r[0] = 0;
            }
            else
            {
                // set v_m to Q_m
                for (i = 0; i < q0.Length; ++i)
                    v[m][i] = point[i] - q0[i];

                // compute the a_{m,i}, i< m
                for (i = 1; i < m; ++i)
                {
                    a[m][i] = 0;
                    for (j = 0; j < a[m].Length; ++j)
                        a[m][i] += v[i][j] * v[m][j];
                    a[m][i] *= (2 / z[i]);
                }

                // update v_m to Q_m-\bar{Q}_m
                for (i = 1; i < m; ++i)
                {
                    for (j = 0; j < a[m].Length; ++j)
                        v[m][j] -= a[m][i] * v[i][j];
                }

                // compute z_m
                z[m] = 0;
                for (j = 0; j < v[m].Length; ++j)
                    z[m] += Sqr(v[m][j]);
                z[m] *= 2;

                // reject push if z_m too small
                if (z[m] < eps * current_sqr_r)
                {
                    return false;
                }

                // update c, sqr_r
                double e = -sqr_r[m - 1];
                for (i = 0; i < point.Length; ++i)
                    e += Sqr(point[i] - c[m - 1][i]);
                f[m] = e / z[m];

                for (i = 0; i < c[m].Length; ++i)
                    c[m][i] = c[m - 1][i] + f[m] * v[m][i];
                sqr_r[m] = sqr_r[m - 1] + e * f[m] / 2;
            }
            current_c = m;
            current_sqr_r = sqr_r[m];
            s = ++m;
            return true;
        }


        public double Slack()
        {
            double[] l = new double[dimensions + 1];
            double min_l = 0;
            l[0] = 1;
            for (int i = s - 1; i > 0; --i)
            {
                l[i] = f[i];
                for (int k = s - 1; k > i; --k)
                    l[i] -= a[k][i] * l[k];
                if (l[i] < min_l) min_l = l[i];
                l[0] -= l[i];
            }
            if (l[0] < min_l) min_l = l[0];
            return ((min_l < 0) ? -min_l : 0);
        }

    }
}
